/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : anotcFlow.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月23日
  ******************************************************************************
  */
/* USER CODE END Header */
#include "tran2MAV.hpp"
#include "string.h"

TRAN2MAV tran2MAV(USART1,(char *)"tran2MAV");

void USART1_IRQHandler(void)
{
	if(LL_USART_IsActiveFlag_IDLE(USART1))
	{
		LL_USART_ClearFlag_IDLE(USART1);

		LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_5);
		LL_DMA_ClearFlag_DME5(DMA2);
		LL_DMA_ClearFlag_HT5(DMA2);
		LL_DMA_ClearFlag_TC5(DMA2);
		LL_DMA_ClearFlag_TE5(DMA2);
		LL_DMA_ClearFlag_FE5(DMA2);
		tran2MAV.RxDataSize = TRAN2MAV_RX_LEN - LL_DMA_GetDataLength(DMA2, LL_DMA_STREAM_5);

		do{

			BaseType_t YieldRequired = xTaskResumeFromISR(tran2MAVReceiveTaskHandle);
			if(YieldRequired==pdTRUE)
			{
				/*如果函数xTaskResumeFromISR()返回值为pdTRUE，那么说明要恢复的这个
				任务的任务优先级等于或者高于正在运行的任务(被中断打断的任务),所以在
				退出中断的时候一定要进行上下文切换！*/
				portYIELD_FROM_ISR(YieldRequired);
			}

		}while(0);

		LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_5, TRAN2MAV_RX_LEN);
		LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_5);
	}
}

void TRAN2MAV::tran2MAV_Init(void)
{
	LockRx = HAL_UNLOCKED;
	RxFlag = false;
	TxFlag = false;
	RxDataSize = 0;
	executionTime_us = 0;
	Sta = STA_INI;
	Err = ERR_NONE;

	/* 配置接收DMA */
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_5);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_5, (uint32_t)&huart->RDR);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_5, (uint32_t)RxRawDat);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_5, TRAN2MAV_RX_LEN);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_5);
	/* 配置接收DMA */

	/* 配置发送DMA */
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_7);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_7, (uint32_t)&huart->TDR);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_7, (uint32_t)TxDat);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_7, TRAN2MAV_TX_LEN);
	LL_DMA_ClearFlag_TC6(DMA2);
	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_7);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_7);
	/* 配置发送DMA */

	LL_USART_EnableDMAReq_RX(huart);
	LL_USART_EnableDMAReq_TX(huart);
	LL_USART_ClearFlag_IDLE(huart);
	LL_USART_EnableIT_IDLE(huart);

	slam.sequence = 0;
	slam.slam_pose[0] = 0;
	slam.slam_pose[1] = 0;
	slam.slam_pose[2] = 0;
	slam.slam_valid_flag = 0;
	xQueueOverwrite(queueSLAM,&slam);
	Sta = STA_RUN;
	osDelay(100);
}

void TRAN2MAV::tran2MAV_Update(void)
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	if(LockRx == HAL_LOCKED)return;
	LockRx = HAL_LOCKED;
	do{
		uint8_t num = RxDat[4]+6;
		RxSum = 0;
		for(uint8_t i=0; i<num-1; i++) RxSum += RxDat[i];
		//if(RxSum != RxDat[num-1])break;		//和校验 判断sum\

	}while(0);
	LockRx = HAL_UNLOCKED;
	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}

bool TRAN2MAV::uart_Send_DMA(uint8_t * pData,uint16_t Size)
{
	if(TxFlag == true) return false;	//串口发送忙,放弃发送该帧数据
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_7);

	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_7, Size);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_7, (uint32_t)pData);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_7);
	TxFlag = true;
	return true;
}

void DMA2_Stream5_IRQHandler(void)  //接收DMA中断
{

}

void DMA2_Stream7_IRQHandler(void)  //发送DMA中断
{
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_7);
	LL_DMA_ClearFlag_TC7(DMA2);
	tran2MAV.TxFlag = false;
}

bool TRAN2MAV::uart_Send_User(void){
	xQueuePeek(queueMagRaw, &magraw, 0);
	xQueuePeek(queueSLAM, &slam, 0);
	if(tran2MAV.TxFlag == true) return false;
	vs16 temp;
	tran2MAV.TxDat[0] = 0xF1;
	tran2MAV.TxDat[1] = 18;

	temp = magraw.mag_raw[0];						//user_data1
	tran2MAV.TxDat[2] = BYTE0(temp);
	tran2MAV.TxDat[3] = BYTE1(temp);

	temp = magraw.mag_raw[1];						//user_data2
	tran2MAV.TxDat[4] = BYTE0(temp);
	tran2MAV.TxDat[5] = BYTE1(temp);

	temp = magraw.mag_raw[2];						//user_data3
	tran2MAV.TxDat[6] = BYTE0(temp);
	tran2MAV.TxDat[7] = BYTE1(temp);

	temp = slam.sequence;
	tran2MAV.TxDat[8] = BYTE0(temp);
	tran2MAV.TxDat[9] = BYTE1(temp);

	temp = slam.slam_pose[0];
	tran2MAV.TxDat[10] = BYTE0(temp);
	tran2MAV.TxDat[11] = BYTE1(temp);

	temp = slam.slam_pose[1];
	tran2MAV.TxDat[12] = BYTE0(temp);
	tran2MAV.TxDat[13] = BYTE1(temp);

	temp = slam.slam_pose[2];
	tran2MAV.TxDat[14] = BYTE0(temp);
	tran2MAV.TxDat[15] = BYTE1(temp);

	tran2MAV.TxDat[16] = tran2MAV.slam.slam_valid_flag;

	uint8_t sum = 0;
	for(uint8_t i=0; i<17; i++) sum += TxDat[i];
	TxDat[17] = sum;
	return uart_Send_DMA((u8 *)TxDat, 18);
}

extern "C" void tran2MAV_main(void *argument)
{
	tran2MAV.tran2MAV_Init();
	osDelay(10);
	for(;;)
	{
		osSemaphoreAcquire(semTran2MAV,0xffffffff);
		tran2MAV.uart_Send_User();
	}

}
extern "C" void tran2MAVReceive_main(void *argument)
{
	osDelay(500);
	for(;;)
	{
		vTaskSuspend(tran2MAVReceiveTaskHandle);
		tran2MAV.tran2MAV_Update();
	}

}




/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
